#include <ocs2_core/dynamics/SystemDynamicsBaseAD.h>
#include <ocs2_pinocchio_interface/PinocchioInterface.h>
#include <memory>
namespace cartpole
{
    using namespace ocs2;
    class CartpoleDynamic : public SystemDynamicsBaseAD
    {
    private:
        /* data */
        std::unique_ptr<PinocchioInterfaceCppAd> pinocchio_interface_ad_ptr_;
        const size_t StateDim = 8;
        const size_t InputDim = 1;

    public:
        ~CartpoleDynamic() override = default;
        CartpoleDynamic(const CartpoleDynamic &rhs) : SystemDynamicsBaseAD(rhs){};

        CartpoleDynamic *clone() const override { return new CartpoleDynamic(*this); }

        CartpoleDynamic(const PinocchioInterface &pinoccho_interface);
        ad_vector_t systemFlowMap(ad_scalar_t time, const ad_vector_t &state, const ad_vector_t &input,
                                  const ad_vector_t &parameters) const override;
    };

}